﻿/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "sixjointrobot.h"
#include <math.h>
#include <stdlib.h>
#include <QDebug>

SixJointRobot::SixJointRobot(void)
{

}

SixJointRobot::~SixJointRobot(void)
{
}

int SixJointRobot::initialForIk(double robotDH[6][4])
{

//初始化ＤＨ
    for(int i=0;i<6;i++)
    {
//        for(int j=0;j<4;j++)
//        {
//            m_DH[i][j]=robotDH[i][j];
//        }
        m_DH[i][0]=robotDH[i][3];
        m_DH[i][1]=robotDH[i][1];
        m_DH[i][2]=robotDH[i][0];
        m_DH[i][3]=robotDH[i][2];

    }


    //计算机器人结构参数
    eox=m_DH[0][2];
    eoy=m_DH[0][3];
    l1=m_DH[1][2];
    l2=sqrt(m_DH[2][2]*m_DH[2][2]+m_DH[3][3]*m_DH[3][3]);
    bJ3=acos(m_DH[2][2]/l2);


    //计算连杆转移矩阵
    calculateLinkMatrix(m_DH[3][0], m_DH[3][1], m_DH[3][2], m_DH[3][3],  A4);
    calculateLinkMatrix(m_DH[4][0], m_DH[4][1], m_DH[4][2], m_DH[4][3],  A5);
    calculateLinkMatrix(m_DH[5][0], m_DH[5][1], m_DH[5][2], m_DH[5][3],  A6);
  //  calculateLinkMatrix(m_DH[6][0], m_DH[6][1], m_DH[6][2], m_DH[6][3],  A7);
   // calculateLiMatrix(A7, A7LI);//转移矩阵的逆矩阵计算

    double MatrixC[4][4];
    multiplymatrix( A4, A5, MatrixC);
    multiplymatrix( MatrixC, A6, A456);

}


int SixJointRobot::getIkConfig_bias( double beforeJoint[6],  int fut[3])
{
//       beforeJoint[1]-=M_PI_2;
//       return getIkConfig(beforeJoint,fut);

}
#define D_DEBUG 1
int SixJointRobot::getIkSolutionFUT(double poseMatrix[4][4], int fut[6],
                                        double jointSolution[6])
{
    //初始化m_T60
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            m_T60[i][j]=poseMatrix[i][j];
        }

    }

    for(int i=0;i<9;i++)//角度有效初始化
    {
        m_JIsValidate[i]=true;
    }

    //计算8组J1
    if(1!=calculateJ1( ))
    {
        if(D_DEBUG)
        {
            qDebug()<<"1!=calculateJ1( )";
        }
        return -1;
    }

    //计算8组J2J3
    if(1!=calculateJ2J3( ))
    {
        if(D_DEBUG)
        {
            qDebug()<<"1!=calculateJ2J3( )";
        }
        return -2;
    }

    if(m_JIsValidate[0]==true)//判断近端是否有解
    {
        calculateEulerTransferMatrix(0);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,0))
        {
            if(D_DEBUG)
            {
                qDebug()<<"1!=calculateEulerJ456( ) 0";
            }
            return -3;
        }

        calculateEulerTransferMatrix(2);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,2))
        {
            if(D_DEBUG)
            {
                qDebug()<<"1!=calculateEulerJ456( ) 2";
            }
            return -3;
        }

    }

    if(m_JIsValidate[4]==true)//判断远端是否有解
    {
        calculateEulerTransferMatrix(4);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,4))
        {
            if(D_DEBUG)
            {
                qDebug()<<"1!=calculateEulerJ456( ) 4";
            }
            return -3;
        }

        calculateEulerTransferMatrix(6);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,6))
        {
            if(D_DEBUG)
            {
                qDebug()<<"1!=calculateEulerJ456( ) 6";
            }
            return -3;
        }

    }

    if(1!=selectBestSolutionFUT(fut, jointSolution))
    {
        if(D_DEBUG)
        {
            qDebug()<<"1!=selectBestSolutionFUT( )";
        }
        return -1;
    }


    return 1;
}

int SixJointRobot::getIkSolutionFUT_bias(double poseMatrix[4][4], int fut[6], double jointSolution[6])
{
    int tmpKey=getIkSolutionFUT(poseMatrix,fut,jointSolution);
    jointSolution[1]+=M_PI_2;
    return tmpKey;
}

int SixJointRobot::getIkSolution(double poseMatrix[4][4], double beforeJoint[6],
                double jointSolution[6],int debugFlag,int &bestSoIndexOut)//求解六轴串联机器人的位置逆解.
{
   // beforeJoint[1]-=M_PI_2;

    //初始化m_T60
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            m_T60[i][j]=poseMatrix[i][j];
        }

    }

	for(int i=0;i<9;i++)//角度有效初始化
	{
        m_JIsValidate[i]=true;
	}

    //计算8组J1
    if(1!=calculateJ1( ))
    {
        return -1;
    }

    //计算8组J2J3
    if(1!=calculateJ2J3( ))
    {
        return -2;
    }

	if(m_JIsValidate[0]==true)//判断近端是否有解
	{
        calculateEulerTransferMatrix(0);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,0))
        {
        return -3;
        }

        calculateEulerTransferMatrix(2);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,2))
        {
            return -30;
        }

	}
	if(m_JIsValidate[4]==true)//判断远端是否有解
	{
        calculateEulerTransferMatrix(4);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,4))
        {
        return -31;
        }


        calculateEulerTransferMatrix(6);//计算第i、i+1组解的欧拉转移矩阵T456

        //计算第i、i+1组解的J4J5J6
        if(1!=calculateEulerJ456(T456,  m_J,6))
        {
            return -32;
        }

	}


    //比较最优解。满足条件１）没有奇异点。２）离初始点最近。
    if(1!=selectBestSolution(beforeJoint, jointSolution,debugFlag,bestSoIndexOut))
    {
        return -133;
    }

    return 1;


}

int SixJointRobot::getIkConfig(double beforeJoint[6], int fut[3])
{
    //beforeJoint[1]-=M_PI;

    double poseMatrix[4][4];
    double jointSolution[6];
    calculatePosition(beforeJoint, poseMatrix);
    int tmpBest;
    int ikSolution = getIkSolution(poseMatrix, beforeJoint, jointSolution,0,tmpBest);
    if (ikSolution < 0)
    {
        return ikSolution;
    }


    if (0 == tmpBest)
    {
        fut[0] = 1;
        fut[1] = 0;
        fut[2] = 1;
    }
    else if (1 == tmpBest)
    {
        fut[0] = 0;
        fut[1] = 0;
        fut[2] = 1;
    }
    else if (2 == tmpBest)
    {
        fut[0] = 1;
        fut[1] = 1;
        fut[2] = 1;
    }
    else if (3 == tmpBest)
    {
        fut[0] = 0;
        fut[1] = 1;
        fut[2] = 1;
    }
    else if (4 == tmpBest)
    {
        fut[0] = 1;
        fut[1] = 0;
        fut[2] = 0;
    }
    else if (5 == tmpBest)
    {
        fut[0] = 0;
        fut[1] = 0;
        fut[2] = 0;
    }
    else if (6 == tmpBest)
    {
        fut[0] = 1;
        fut[1] = 1;
        fut[2] = 0;
    }
    else if (7 == tmpBest)
    {
        fut[0] = 0;
        fut[1] = 1;
        fut[2] = 0;
    }
    else
    {
        qDebug()<<"error,,,tmpBest unknow";
    }


//    if (1 == m_bestSolutionIndex)
//    {
//        fut[0] = 1;
//        fut[1] = 0;
//        fut[2] = 1;
//    }
//    else if (0 == m_bestSolutionIndex)
//    {
//        fut[0] = 0;
//        fut[1] = 0;
//        fut[2] = 1;
//    }
//    else if (3 == m_bestSolutionIndex)
//    {
//        fut[0] = 1;
//        fut[1] = 1;
//        fut[2] = 1;
//    }
//    else if (2 == m_bestSolutionIndex)
//    {
//        fut[0] = 0;
//        fut[1] = 1;
//        fut[2] = 1;
//    }
//    else if (5 == m_bestSolutionIndex)
//    {
//        fut[0] = 1;
//        fut[1] = 0;
//        fut[2] = 0;
//    }
//    else if (4 == m_bestSolutionIndex)
//    {
//        fut[0] = 0;
//        fut[1] = 0;
//        fut[2] = 0;
//    }
//    else if (7 == m_bestSolutionIndex)
//    {
//        fut[0] = 1;
//        fut[1] = 1;
//        fut[2] = 0;
//    }
//    else if (6 == m_bestSolutionIndex)
//    {
//        fut[0] = 0;
//        fut[1] = 1;
//        fut[2] = 0;
//    }
    return 1;
}

int SixJointRobot::getIkSolution_bias(double poseMatrix[4][4], double beforeJoint[6],
                                        double jointSolution[6])
{
//    beforeJoint[1]-=M_PI_2;
//    int tmpkey=getIkSolution(poseMatrix,beforeJoint,jointSolution,0);
//    jointSolution[1]+=M_PI_2;
//    return tmpkey;
    return 0;
}

void SixJointRobot::multiplymatrix(double a[][4], double b[][4],double c[][4])//矩阵乘法函数
{
  c[0][0]=a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];
  c[1][0]=a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];
  c[2][0]=a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];
  c[3][0]=a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];

  c[0][1]=a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];
  c[1][1]=a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];
  c[2][1]=a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];
  c[3][1]=a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];

  c[0][2]=a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];
  c[1][2]=a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];
  c[2][2]=a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];
  c[3][2]=a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];

  c[0][3]=a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];
  c[1][3]=a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];
  c[2][3]=a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];
  c[3][3]=a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];
}

void SixJointRobot::calculateLinkMatrix(double oi, double afi, double ai, double di, double Ai[][4])//连杆转移矩阵计算
{

	Ai[0][0]=cos(oi);
	Ai[0][1]=-sin(oi)*cos(afi);
	Ai[0][2]=sin(oi)*sin(afi);
	Ai[0][3]=ai*cos(oi);

	Ai[1][0]=sin(oi);
	Ai[1][1]=cos(oi)*cos(afi);
	Ai[1][2]=-cos(oi)*sin(afi);
	Ai[1][3]=ai*sin(oi);

	Ai[2][0]=0;
	Ai[2][1]=sin(afi);
	Ai[2][2]=cos(afi);
	Ai[2][3]=di;

	Ai[3][0]=0;
	Ai[3][1]=0;
	Ai[3][2]=0;
	Ai[3][3]=1;

}

void SixJointRobot::calculateLiMatrix(double a[][4], double b[][4])//转移矩阵的逆矩阵计算
{
	b[0][0]=a[0][0];
	b[0][1]=a[1][0];
	b[0][2]=a[2][0];
	b[0][3]=-(a[0][0]*a[0][3]+a[1][0]*a[1][3]+a[2][0]*a[2][3]);

	b[1][0]=a[0][1];
	b[1][1]=a[1][1];
	b[1][2]=a[2][1];
	b[1][3]=-(a[0][1]*a[0][3]+a[1][1]*a[1][3]+a[2][1]*a[2][3]);

	b[2][0]=a[0][2];
	b[2][1]=a[1][2];
	b[2][2]=a[2][2];
	b[2][3]=-(a[0][2]*a[0][3]+a[1][2]*a[1][3]+a[2][2]*a[2][3]);

	b[3][0]=0;
	b[3][1]=0;
	b[3][2]=0;
	b[3][3]=1;
	
}

int SixJointRobot::calculateEulerJ456(double a[][4], double J[][6],int i)//用欧拉法,计算第i、i+1组解的J4J5J6
{


	J[i][4]=atan2(sqrt(a[2][0]*a[2][0]+a[2][1]*a[2][1]),a[2][2]);//返回[-pi, pi]角度.

    //当J5=0时。实际应用可以取0.01°的精度。用0°时的2种解
	if(J[i][4]>-0.01*M_PI/180 && J[i][4]<0.01*M_PI/180 )
	{
        //qDebug()<<"six robot ik solution failed: J5 singuraty!";
        return -3;

//		double sum=atan2(-a[0][1],a[0][0]);
//		//计算第i组解的J4J5J6***********************************
//		J[i][3]=m_bestDHJ[3];
//		J[i][4]=0;
//		J[i][5]=sum-m_bestDHJ[3];//用上次最佳解计算
//		//计算第i+1组解的J4J5J6***********************************
//		J[i+1][3]=sum-m_bestDHJ[5];
//		J[i+1][4]=0;
//		J[i+1][5]=m_bestDHJ[5];//用上次最佳解计算

	}
	//当J5=180°舍弃。
	else if(  J[i][4]>M_PI-0.01*M_PI/180 || J[i][4]<-M_PI+0.01*M_PI/180 )
	{
		//若为180°，则J4-J6=atan2(a[0][1],-a[0][0])
		m_JIsValidate[i]=false;
		m_JIsValidate[i+1]=false;

        //qDebug()<<"six robot ik solution failed: J5 singuraty!";
        return -3;

	}
	else//当J5不等于0或180°时
	{
		//计算第i组解的J4J5J6***********************************
		J[i][3]=atan2(a[1][2]/sin(J[i][4]),a[0][2]/sin(J[i][4]));//返回[-pi, pi]角度.
		J[i][4]=J[i][4];
		J[i][5]=atan2(a[2][1]/sin(J[i][4]),-a[2][0]/sin(J[i][4]));//返回[-pi, pi]角度.

		//计算第i+1组解的J4J5J6***********************************
		J[i+1][3]=J[i][3]+M_PI;
		J[i+1][4]=-J[i][4];
		J[i+1][5]=J[i][5]+M_PI;

	}
	

    return 1;

}


void SixJointRobot::calculatePosition(double DH[][4], double J[][6], int i, double P[][4])//位置姿态运动学正解
{

	//连杆转移矩阵
    double A1[4][4],A2[4][4],A3[4][4],A4[4][4],A5[4][4],A6[4][4],c[4][4],d[4][4];

	calculateLinkMatrix(J[i][0], DH[0][1], DH[0][2], DH[0][3],  A1);
	calculateLinkMatrix(J[i][1], DH[1][1], DH[1][2], DH[1][3],  A2);
	calculateLinkMatrix(J[i][2], DH[2][1], DH[2][2], DH[2][3],  A3);
	calculateLinkMatrix(J[i][3], DH[3][1], DH[3][2], DH[3][3],  A4);
	calculateLinkMatrix(J[i][4], DH[4][1], DH[4][2], DH[4][3],  A5);
	calculateLinkMatrix(J[i][5], DH[5][1], DH[5][2], DH[5][3],  A6);
    //calculateLinkMatrix(m_DH[6][0], m_DH[6][1], m_DH[6][2], m_DH[6][3],  A7);

	multiplymatrix( A1, A2, c);
	multiplymatrix( c, A3, d);
	multiplymatrix( d, A4, c);
	multiplymatrix( c, A5, d);
	multiplymatrix( d, A6, c);
	//multiplymatrix( d, A6, P);
    //multiplymatrix( c, A7, P);

}

int SixJointRobot::calculatePosition(double jointValue[6], double poseMatrix[][4])
{
    //连杆转移矩阵
    double A1[4][4],A2[4][4],A3[4][4],A4[4][4],A5[4][4],A6[4][4],c[4][4],d[4][4];

    calculateLinkMatrix(jointValue[0], m_DH[0][1], m_DH[0][2], m_DH[0][3],  A1);
//    jointValue[1] += M_PI_2;
    double tmpJ2=jointValue[1] + M_PI_2;
    calculateLinkMatrix(tmpJ2, m_DH[1][1], m_DH[1][2], m_DH[1][3],  A2);
    calculateLinkMatrix(jointValue[2], m_DH[2][1], m_DH[2][2], m_DH[2][3],  A3);
    calculateLinkMatrix(jointValue[3], m_DH[3][1], m_DH[3][2], m_DH[3][3],  A4);
    calculateLinkMatrix(jointValue[4], m_DH[4][1], m_DH[4][2], m_DH[4][3],  A5);
    calculateLinkMatrix(jointValue[5], m_DH[5][1], m_DH[5][2], m_DH[5][3],  A6);

	multiplymatrix( A1, A2, c);
	multiplymatrix( c, A3, d);
	multiplymatrix( d, A4, c);
	multiplymatrix( c, A5, d);
    multiplymatrix( d, A6, poseMatrix);

    return 1;
}


int SixJointRobot::calculateJ1(void)//计算8组J1
{

    //判断是否有顶置奇异点
    if( (m_T60[1][3]<0.02 && m_T60[1][3]>-0.02)
            && (m_T60[0][3]<0.02 && m_T60[0][3]>-0.02))
    {
        //qDebug()<<"six robot ik solution failed: top singuraty!";
        return -1;
    }
	m_J[0][0]=atan2(m_T60[1][3],m_T60[0][3]);//nearJ1  返回[-pi, pi]角度.
	m_J[1][0]=m_J[0][0];//nearJ1
	m_J[2][0]=m_J[0][0];//nearJ1
	m_J[3][0]=m_J[0][0];//nearJ1

	m_J[4][0]=m_J[0][0]+M_PI;//farJ1
	m_J[5][0]=m_J[4][0];//farJ1
	m_J[6][0]=m_J[4][0];//farJ1
	m_J[7][0]=m_J[4][0];//farJ1

    return 1;


}

int SixJointRobot::calculateJ2J3(void)//计算8组J2J3
{
	//当J1为近位置时*******
    double ox=sqrt(m_T60[1][3]*m_T60[1][3]+m_T60[0][3]*m_T60[0][3]);
    double oy=m_T60[2][3];
    double x1=ox-eox;
    double y1=oy-eoy;
    double b=atan2(y1,x1);//弦角度 返回[-pi, pi]角度.
    double angleRange=(x1*x1+y1*y1+l1*l1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1));
    double a;

    if(angleRange>=-0.999 && angleRange<=0.999)//检验目标点是否超出活动范围,离手臂成直线还差?度
	{
		a=acos(angleRange);//增减角，返回[0,180]范围内的角度
		m_J[0][1]=b-a;      //小J2，对应正的J3角度
		m_J[1][1]=m_J[0][1];//小J2，对应正的J3角度

		m_J[2][1]=b+a;      //大J2,对应负的J3角度
		m_J[3][1]=m_J[2][1];//大J2,对应负的J3角度

		m_J[0][2]=acos((x1*x1+y1*y1-l1*l1-l2*l2)/(2*l1*l2));//正J3
		m_J[1][2]=m_J[0][2];                                //正J3

		m_J[2][2]=-m_J[1][2];//负J3
		m_J[3][2]=m_J[2][2]; //负J3
	}
	else
	{
		m_JIsValidate[0]=false;
		m_JIsValidate[1]=false;
		m_JIsValidate[2]=false;
		m_JIsValidate[3]=false;

	}


    //当J1为远位置时********
	x1=ox+eox;
	b=atan2(y1,x1);
	angleRange=(x1*x1+y1*y1+l1*l1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1));

    if(angleRange>=-0.999 && angleRange<=0.999)//检验目标点是否超出活动范围
	{
		a=acos(angleRange);//增减角
		m_J[4][1]=M_PI-(b+a);//小J2，对应正的J3角度
		m_J[5][1]=m_J[4][1];//小J2，对应正的J3角度

		m_J[6][1]=M_PI-(b-a);//大J2,对应负的J3角度
		m_J[7][1]=m_J[6][1];//大J2,对应负的J3角度

		m_J[4][2]=acos((x1*x1+y1*y1-l1*l1-l2*l2)/(2*l1*l2));//正J3
		m_J[5][2]=m_J[4][2];                                //正J3

		m_J[6][2]=-m_J[5][2];//负J3
		m_J[7][2]=m_J[6][2]; //负J3

	}
	else
	{
		m_JIsValidate[4]=false;
		m_JIsValidate[5]=false;
		m_JIsValidate[6]=false;
		m_JIsValidate[7]=false;

	}

    // J3 singuraty!
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false && m_JIsValidate[2]==false &&m_JIsValidate[3]==false &&
            m_JIsValidate[4]==false && m_JIsValidate[5]==false && m_JIsValidate[6]==false &&m_JIsValidate[7]==false )
    {
        //qDebug()<<"six robot ik solution failed: J3 singuraty!";
        return -2;
    }


	m_J[0][2]=m_J[0][2]+bJ3;//更新J3为标准DH角度
	m_J[1][2]=m_J[1][2]+bJ3;//更新J3为标准DH角度
	m_J[2][2]=m_J[2][2]+bJ3;//更新J3为标准DH角度
	m_J[3][2]=m_J[3][2]+bJ3;//更新J3为标准DH角度
	m_J[4][2]=m_J[4][2]+bJ3;//更新J3为标准DH角度
	m_J[5][2]=m_J[5][2]+bJ3;//更新J3为标准DH角度
	m_J[6][2]=m_J[6][2]+bJ3;//更新J3为标准DH角度
	m_J[7][2]=m_J[7][2]+bJ3;//更新J3为标准DH角度

    return 1;

}

void SixJointRobot::calculateEulerTransferMatrix(int i)//第i组解的连杆转移矩阵
{
		
	calculateLinkMatrix(m_J[i][0], m_DH[0][1], m_DH[0][2], m_DH[0][3],  A1);
	calculateLinkMatrix(m_J[i][1], m_DH[1][1], m_DH[1][2], m_DH[1][3],  A2);
	calculateLinkMatrix(m_J[i][2], m_DH[2][1], m_DH[2][2], m_DH[2][3],  A3);

    double MatrixC[4][4];
	multiplymatrix( A1, A2, MatrixC);
	multiplymatrix( MatrixC, A3, A123);
	multiplymatrix( A123, A456, A1_6);//求得A1*A2*A3*A4*A5*A6

	calculateLiMatrix(A1_6, A1_6LI);//求A1_6的逆矩阵

    multiplymatrix( A1_6LI, m_T60, T456);//求得第2组解的连杆456的目标转移矩阵T456，位置元素误差百分比大

}

void SixJointRobot::MatrixRotationXself(double Matrixa[][4], double angle)
{
    double a[4][4]={{1,0,0,0}, {0,cos(angle),-sin(angle),0}, { 0,sin(angle),cos(angle),0}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void SixJointRobot::MatrixRotationYself(double Matrixa[][4], double angle)
{
    double a[4][4]={{cos(angle),0,sin(angle),0}, {0,1,0,0}, { -sin(angle),0,cos(angle),0}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}

void SixJointRobot::MatrixRotationZself(double Matrixa[][4], double angle)
{
    double a[4][4]={{cos(angle),-sin(angle),0,0}, {sin(angle),cos(angle),0,0}, { 0,0,1,0}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void SixJointRobot::MatrixTranslateSelfcoordinate(double Matrixa[][4],double x,double y,double z)//参考TCP坐标平移
{
    double a[4][4]={{1,0,0,x}, {0,1,0,y}, { 0,0,1,z}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void SixJointRobot::MatrixTranslateWorldcoordinate(double Matrixa[][4],double x,double y,double z)//参考世界坐标平移
{
	Matrixa[0][3]+=x;
	Matrixa[1][3]+=y;
	Matrixa[2][3]+=z;
}


void SixJointRobot::MatrixEqual(double a[][4], double b[][4])//b<=a
{
for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			b[i][j]=a[i][j];
		  
		}
	  
	}
}

void SixJointRobot::MatrixEqual(double a[][4], double b[][4],int n)
{
for(int i=0;i<n;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			b[i][j]=a[i][j];
		  
		}
	  
	}
}


void SixJointRobot::TransferDirectJToDHJ(double DJ[6], double DHJ[6],bool IsDirectAngle2DHJAngle)
{
	  if(IsDirectAngle2DHJAngle)
	  {
	  DHJ[0]=-DJ[0];
      DHJ[1]=DJ[1]+M_PI_2;
	  DHJ[2]=-DJ[2];
	  DHJ[3]=DJ[3];
	  DHJ[4]=-DJ[4];
	  DHJ[5]=-DJ[5];
	  }
	  else
	  {
	  DJ[0]=-DHJ[0];
      DJ[1]=DHJ[1]-M_PI_2;
	  DJ[2]=-DHJ[2];
	  DJ[3]=DHJ[3];
	  DJ[4]=-DHJ[4];
	  DJ[5]=-DHJ[5];

	  }


}
void SixJointRobot::TransferDirectJToDHJ(double DJ[6], double DHJ1,double DHJ2,double DHJ3,double DHJ4,double DHJ5,double DHJ6,bool IsDirectAngle2DHJAngle)
{
	  if(IsDirectAngle2DHJAngle)
	  {
	  DHJ1=-DJ[0];
      DHJ2=DJ[1]+M_PI_2;
	  DHJ3=-DJ[2];
	  DHJ4=DJ[3];
	  DHJ5=-DJ[4];
	  DHJ6=-DJ[5];
	  }
	  else
	  {
	  DJ[0]=-DHJ1;
	  DJ[1]=DHJ2-M_PI_2;
	  DJ[2]=-DHJ3;
	  DJ[3]=DHJ4;
	  DJ[4]=-DHJ5;
	  DJ[5]=-DHJ6;

	  }


}

int SixJointRobot::selectBestSolutionFUT(int fut[6], double jointSolution[6])
{
    //判断是否存在位置不可达到的情况
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false
            &&m_JIsValidate[2]==false && m_JIsValidate[3]==false
            &&m_JIsValidate[4]==false && m_JIsValidate[5]==false
            &&m_JIsValidate[6]==false && m_JIsValidate[7]==false)
    {
        qDebug()<<"six joint robot get is solution failed: exceed arm length!";
        return -1; //位置不可达
    }

    //第二关节减去９０度,第二轴以垂直状态为０度
    for(int n=0;n<8;n++)
    {
        m_J[n][1] -= M_PI_2;
    }

    //所有解的角度表示转换，使在[-pi,pi]区间
    int n,m;
    for(n=0;n<8;n++)
    {
        for(m=0;m<6;m++)
        {
           if(m_J[n][m]>M_PI)
           {
               m_J[n][m]=m_J[n][m]-M_PI*2;
           }
           if(m_J[n][m]<-M_PI)
           {
               m_J[n][m]=m_J[n][m]+M_PI*2;
           }

        }
    }
    int bestIndex = 0;
    if(fut[0] == 1 && fut[1] == 1 && fut[2] == 1)//手腕上（1）下，肘部上（1）下，基座前（1）后
    {
        bestIndex = 2;
    }
    else if(fut[0] == 0 && fut[1] == 1 && fut[2] == 1)
    {
        bestIndex = 3;
    }
    else if(fut[0] == 1 && fut[1] == 0 && fut[2] == 1)
    {
        bestIndex = 0;
    }
    else if(fut[0] == 0 && fut[1] == 0 && fut[2] == 1)
    {
        bestIndex = 1;
    }
    else if(fut[0] == 1 && fut[1] == 1 && fut[2] == 0)
    {
        bestIndex = 6;
    }
    else if(fut[0] == 0 && fut[1] == 1 && fut[2] == 0)
    {
        bestIndex = 7;
    }
    else if(fut[0] == 1 && fut[1] == 0 && fut[2] == 0)
    {
        bestIndex = 4;
    }
    else if(fut[0] == 0 && fut[1] == 0 && fut[2] == 0)
    {
        bestIndex = 5;
    }
    else
    {
        qDebug()<<"error,,,unknow fut"<<fut[0]<<fut[1]<<fut[2];
    }
//    if(fut[0] == 1 && fut[1] == 1 && fut[2] == 1)//手腕上（1）下，肘部上（1）下，基座前（1）后
//    {
//        bestIndex = 3;
//    }
//    else if(fut[0] == 0 && fut[1] == 1 && fut[2] == 1)
//    {
//        bestIndex = 2;
//    }
//    else if(fut[0] == 1 && fut[1] == 0 && fut[2] == 1)
//    {
//        bestIndex = 1;
//    }
//    else if(fut[0] == 0 && fut[1] == 0 && fut[2] == 1)
//    {
//        bestIndex = 0;
//    }
//    else if(fut[0] == 1 && fut[1] == 1 && fut[2] == 0)
//    {
//        bestIndex = 7;
//    }
//    else if(fut[0] == 0 && fut[1] == 1 && fut[2] == 0)
//    {
//        bestIndex = 6;
//    }
//    else if(fut[0] == 1 && fut[1] == 0 && fut[2] == 0)
//    {
//        bestIndex = 5;
//    }
//    else if(fut[0] == 0 && fut[1] == 0 && fut[2] == 0)
//    {
//        bestIndex = 4;
//    }

//    m_bestSolutionIndex=bestIndex;

    if (m_JIsValidate[bestIndex] == false)
    {
        if(D_DEBUG)
        {
            qDebug()<<"m_JIsValidate[bestIndex] == false bestIndex"<<bestIndex<<"fut"<<fut[0]<<fut[1]<<fut[2]
                   <<"m_JIsValidate"<<m_JIsValidate[0]<<m_JIsValidate[1]<<m_JIsValidate[2]<<m_JIsValidate[3]
                     <<m_JIsValidate[4]<<m_JIsValidate[5]<<m_JIsValidate[6]<<m_JIsValidate[7];
        }
        return -1;
    }

    //最优解赋值
    for(int i=0;i<6;i++)
    {
        jointSolution[i]=m_J[bestIndex][i];
    }
    //多圈处理
    jointSolution[0] += fut[3] * M_PI * 2;
    jointSolution[3] += fut[4] * M_PI * 2;
    jointSolution[5] += fut[5] * M_PI * 2;
    return 1;
}


int SixJointRobot::getTurnsFromDegree(double degreeIn)
{
    //    int turns;
    //    degreeIn = (degreeIn+180.0) /360.0;
    //    if(degreeIn>0.0){
    //        turns = floor(degreeIn);//往下取整
    //    }
    //    else{
    //        turns = ceil(degreeIn);//往上取整
    //    }

        return getTurnsFromRad(degreeIn/180.0*M_PI);
}

int SixJointRobot::getTurnsFromRad(double degreeIn)
{
    int turns;
//    degreeIn = (degreeIn+M_PI) /(2*M_PI);
//    if(degreeIn>0.0){
//        turns = floor(degreeIn);//往下取整
//    }
//    else{
//        turns = ceil(degreeIn);//往上取整
//    }
    if(0<degreeIn)
    {
        turns=(degreeIn+M_PI) /(2*M_PI);
    }
    else
    {
        turns=(degreeIn-M_PI) /(2*M_PI);
    }

    return turns;
}

double SixJointRobot::getTrimDegreeFromDegree(double degreeIn)
{
    int turns=getTurnsFromDegree(degreeIn);
    double trimDegree=degreeIn-360.0*turns;
    return trimDegree;

}

double SixJointRobot::getTrimRadFromRad(double degreeIn)
{
    int turns=getTurnsFromRad(degreeIn);
    double trimDegree=degreeIn-2*M_PI*turns;
    return trimDegree;

}

int SixJointRobot::selectBestSolution(double beforeJoint[6], double jointSolution[6],int debugFlag,int &bestSoIndexOut)
{
    bestSoIndexOut=-1;
    //先除去多圈，用于选解 第一、四、六轴的多圈
//    int jointTurn[6];
//    jointTurn[0]=getTurnsFromRad(beforeJoint[0]);
//    jointTurn[3]=getTurnsFromRad(beforeJoint[3]);
//    jointTurn[5]=getTurnsFromRad(beforeJoint[5]);
//    double originJoint[6];
//    originJoint[0]=beforeJoint[0];
//    originJoint[3]=beforeJoint[3];
//    originJoint[5]=beforeJoint[5];
//    beforeJoint[0]-=jointTurn[0]*2*M_PI;
//    beforeJoint[3]-=jointTurn[3]*2*M_PI;
//    beforeJoint[5]-=jointTurn[5]*2*M_PI;

    //判断是否存在位置不可达到的情况
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false
            &&m_JIsValidate[2]==false && m_JIsValidate[3]==false
            &&m_JIsValidate[4]==false && m_JIsValidate[5]==false
            &&m_JIsValidate[6]==false && m_JIsValidate[7]==false)
    {
        qDebug()<<"six joint robot get is solution failed: exceed arm length!";
        return -1; //位置不可达
    }

    //第二关节减去９０度,第二轴以垂直状态为０度
    for(int n=0;n<8;n++)
    {
        m_J[n][1] -= M_PI_2;
    }

	//所有解的角度表示转换，使在[-pi,pi]区间
    int n,m;
	for(n=0;n<8;n++)
	{
		for(m=0;m<6;m++)
		{
		   if(m_J[n][m]>M_PI)
		   {
			   m_J[n][m]=m_J[n][m]-M_PI*2;
		   }
		   if(m_J[n][m]<-M_PI)
		   {
			   m_J[n][m]=m_J[n][m]+M_PI*2;
		   }
		  
		}
			  
	}

//     // 满足条件１）没有奇异点。２）离初始点最近。

//        //计算各组解的J1+J2+J3+J5变化绝对值和
//        double JChangedSum[8]={0,0,0,0,0,0,0,0};
//		for( n=0;n<8;n++)
//		{
//            JChangedSum[n]=fabs(m_J[n][0]-beforeJoint[0])+fabs(m_J[n][1]-beforeJoint[1])+fabs(m_J[n][2]-beforeJoint[2])
//                           +fabs(m_J[n][4]-beforeJoint[4]);
//		}

//        //探测有效解的第一组
//        int bestIndex=0;
//		for( n=0;n<8;n++)
//		{
//			if(m_JIsValidate[n]==true)
//			{
//				bestIndex=n;
//				break;
				
//			}
//		}


//        //循环比较角度变化最小值。
//		for( n=0;n<8;n++)
//		{
//			if(JChangedSum[n]<JChangedSum[bestIndex] && m_JIsValidate[n]==true)
//			{
//				bestIndex=n;
//			}
			
//		}
//        m_bestSolutionIndex=bestIndex;

//        //最优解赋值
//        for(int i=0;i<6;i++)
//        {
//            jointSolution[i]=m_J[bestIndex][i];
//        }

//        //多圈,其余轴不支持多圈
////        jointSolution[0]+=jointTurn[0]*2*M_PI;
////        jointSolution[2]+=jointTurn[2]*2*M_PI;
////        jointSolution[5]+=jointTurn[5]*2*M_PI;
//        getBestPositionFromAngle_Rad(jointSolution[0],originJoint[0],jointSolution[0]);
//        getBestPositionFromAngle_Rad(jointSolution[3],originJoint[3],jointSolution[3]);
//        getBestPositionFromAngle_Rad(jointSolution[5],originJoint[5],jointSolution[5]);

//        return 1;

    if(debugFlag)
    {
        qDebug()<<"beforeJoint"<<beforeJoint[0]<<beforeJoint[1]<<beforeJoint[2]
                  <<beforeJoint[3]<<beforeJoint[4]<<beforeJoint[5];
    }


    QVector<QVector<double>> tmpJoint;
    int solveNum=8;
    tmpJoint.resize(solveNum);
    double biasSum[solveNum];
    for(int i=0;i<solveNum;i++)
    {
        tmpJoint[i].resize(6);
        if(false==m_JIsValidate[i])
        {
            biasSum[i]=100000;
//            continue;
        }
        else
        {
            //支持多圈
            biasSum[i]=0;
            for(int j=0;j<6;j++)
            {
                getBestPositionFromAngle_Rad(m_J[i][j],beforeJoint[j],tmpJoint[i][j]);
                if(3!=j && 5!=j)
                {
                    biasSum[i]+=fabs(beforeJoint[j]-tmpJoint[i][j]);
                }

            }
        }
        if(debugFlag)
        {
            qDebug()<<"i"<<i<<"biasSum"<<biasSum[i]<<"matchjoint"<<tmpJoint[i][0]<<tmpJoint[i][1]<<tmpJoint[i][2]
                   <<tmpJoint[i][3]<<tmpJoint[i][4]<<tmpJoint[i][5];
        }


    }
    //smallest
    int index=0;
    double tmpSmall=biasSum[0];
    for(int i=1;i<solveNum;i++)
    {
        if(biasSum[i]<tmpSmall)
        {
            tmpSmall=biasSum[i];
            index=i;
        }

    }
    if(debugFlag)
    {
        qDebug()<<"best index"<<index;

    }
    bestSoIndexOut=index;
    if(0)
    {
//        qDebug()<<"---------------------------------solveNum"<<solveNum<<"index"<<index
//               <<"q_initNew"<<q_initNew(0)<<q_initNew(1)<<q_initNew(2)<<q_initNew(3)<<q_initNew(4)<<q_initNew(5);

//        for(int i=0;i<solveNum;i++)
//        {
//            qDebug()<<i <<" qOut"<<qOut[0+i*6]<<qOut[1+i*6]
//                   <<qOut[2+i*6]<<qOut[3+i*6]<<qOut[4+i*6]<<qOut[5+i*6];
//            qDebug()<<i <<" biasSum"<<biasSum[i]<<"tmpJoint"<<tmpJoint[i](0)<<tmpJoint[i](1)
//                      <<tmpJoint[i](2)<<tmpJoint[i](3)<<tmpJoint[i](4)<<tmpJoint[i](5)
//                      <<tmpJoint[i](6);

//        }
    }


    for(int i=0;i<6;i++)
    {
        jointSolution[i]=tmpJoint[index][i];
    }
    return 1;


}

int SixJointRobot::getBestPositionFromAngle_Rad(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int tmpKey= getBestPositionFromAngle( idealAngle/M_PI*180.0, initialPosition/M_PI*180.0, bestPositionOut);
    bestPositionOut=bestPositionOut/180.0*M_PI;
    return tmpKey;
}


int SixJointRobot::getBestPositionFromAngle(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int currentTurns;
    if(0<initialPosition)
    {
        currentTurns=(initialPosition+180) /(360);
    }
    else
    {
        currentTurns=(initialPosition-180) /(360);
    }

    double angle1,angle2,angle3;
    angle1=currentTurns*360+idealAngle;
    angle2=angle1+360;
    angle3=angle1-360;
    double offset1,offset2,offset3;
    double minNum;
    int minIndex;
    offset1=fabs(angle1-initialPosition);
    offset2=fabs(angle2-initialPosition);
    offset3=fabs(angle3-initialPosition);
    if(offset1<=offset2)
    {
        minNum=offset1;
        minIndex=1;
    }
    else
    {
        minNum=offset2;
        minIndex=2;
    }

    if(offset3<=minNum)
    {
        minNum=offset3;
        minIndex=3;
    }
    switch(minIndex)
    {
    case 1:
    {
        bestPositionOut=angle1;
        break;

    }
    case 2:
    {
        bestPositionOut=angle2;
        break;

    }
    default:
    {
        bestPositionOut=angle3;
    }
    }


    return 1;
}


double SixJointRobot::MaxCompare(double x, double y, double z)
{
         double p,q;
		 
		 if(x<y) p=y;
		 else p=x;
		 
		 if(z<p) q = p;
		 else q = z;
		 return q;
}









